package com.dmatek.uwb.local.algorithm;

import java.util.Map;
import com.dmatek.uwb.packet.bean.AssistLocation;
import com.dmatek.uwb.packet.bean.FilterTagInfor;
import com.dmatek.uwb.packet.bean.KalmanFilter;
import com.dmatek.uwb.util.tool.StringUtil;
import com.dmatek.uwb.xml.dom.bean.BasePath;
import com.dmatek.uwb.xml.dom.bean.BranchPath;
import com.dmatek.uwb.xml.dom.bean.Config;
import com.dmatek.uwb.xml.dom.bean.MainPath;
import com.dmatek.uwb.xml.dom.bean.Point;

public class FilterHopsSingleLocalModel implements IFilterHopsModel {
	private boolean isFilter = true;
	private double filterArg = 0.5;
	private Config config;
	private Map<String, BasePath> pathsMap;
	public FilterHopsSingleLocalModel(Config config,
			Map<String, BasePath> pathsMap)
			throws Exception {
		super();
		this.config = config;
		this.pathsMap = pathsMap;
		this.isFilter = config.isFilter();
		this.filterArg = config.getFilterArg();
	}
	/***
	 * 获取跳点的阀值
	 * @param sleep_100ms
	 * @return
	 */
	private final int obtainJumpsThreshold(int sleep_100ms) {
        if (sleep_100ms >= 10) { // 当休眠时间大于1s默认跳点次数为1次
            return 1;
        }
        if (sleep_100ms <= 0) { // 我们获取到休眠时间失败
            return 1;
        }
        return 10 / sleep_100ms;
    }
	/***
	 * 房间->房间跳转
	 * @param nowPath
	 * @param lastPath
	 * @param jumpsThreshold
	 * @param tagInfor
	 * @param place
	 * @return
	 * @throws Exception
	 */
	private final Point branchToBranchPath(BasePath nowPath, BasePath lastPath, int jumpsThreshold, FilterTagInfor tagInfor, Point place) throws Exception {
		double x = -1, y = -1;
		if (lastPath.getId().equals(nowPath.getId())) { // 说明两次路径相同
            if (isFilter()) {
                if (null != tagInfor.getKx() && null != tagInfor.getKy()) {
              	  x = tagInfor.getKx().filter(place.getX());
              	  y = tagInfor.getKy().filter(place.getY());
                } else {
              	  tagInfor.setKx(new KalmanFilter(place.getX(), getFilterArg()));
              	  tagInfor.setKy(new KalmanFilter(place.getY(), getFilterArg()));
              	  // 此时x, y 坐标即为place
                }
            } else {
            	// 此时x, y 坐标即为place
            }
        } else { // 说明两次路径不相同
            if (isSkip(lastPath, nowPath.getId())) {// 可以直接进行跳转
          	  tagInfor.setPid(nowPath.getId());
          	  tagInfor.setPhops(jumpsThreshold);
          	  x = nowPath.getStart().getX();
          	  y = nowPath.getStart().getY();
              if (isFilter()) {
              	  tagInfor.setKx(new KalmanFilter(x, getFilterArg()));
              	  tagInfor.setKy(new KalmanFilter(y, getFilterArg()));
              }
            } else {// 不能直接进行跳转
          	  if(tagInfor.getPhops() <= 0) {
          		  tagInfor.setPid(nowPath.getId());
          		  tagInfor.setPhops(jumpsThreshold);
          		  x = nowPath.getStart().getX();
          		  y = nowPath.getStart().getY();
          		  if(isFilter()) {
          			  tagInfor.setKx(new KalmanFilter(x, getFilterArg()));
          			  tagInfor.setKy(new KalmanFilter(y, getFilterArg()));
          		  }
          	  } else {
          		  tagInfor.setPhops(tagInfor.getPhops() - 1);
          		  return new Point(-1, -1);
          	  }
           }
        }
		if(x <= 0 || y <= 0) {
			return place;
		}
		return new Point(x, y);
	}
	/***
	 * 房间-> 走廊跳转
	 * @param nowPath
	 * @param lastPath
	 * @param jumpsThreshold
	 * @param tagInfor
	 * @param place
	 * @return
	 * @throws Exception
	 */
	private final Point branchToMainPath(BasePath nowPath, BasePath lastPath, int jumpsThreshold, FilterTagInfor tagInfor, Point place) throws Exception {
		double x = -1, y = -1;
		if (tagInfor.getPhops() <= 0) {
      	  tagInfor.setPid(nowPath.getId());
      	  tagInfor.setPhops(jumpsThreshold);
          if (isFilter()) {
          	  tagInfor.setKx(new KalmanFilter(place.getX(), getFilterArg()));
          	  tagInfor.setKy(new KalmanFilter(place.getY(), getFilterArg()));
          }
            // 跳转到主路径后坐标就是计算出来的坐标
        } else {
        	// 没有达到跳转的阀值，当前还是不允许跳转，坐标为开始路径坐标
      	  	tagInfor.setPhops(tagInfor.getPhops() - 1);
            x = lastPath.getStart().getX();
            y = lastPath.getStart().getY();
            if (isFilter()) {
                if (null != tagInfor.getKx() && null != tagInfor.getKy()) {
                    x = tagInfor.getKx().filter(x);;
                    y = tagInfor.getKy().filter(y);;
                }
            }
        }
		if(x <= 0 || y <= 0) {
			return place; 
		}
		return new Point(x, y);
	}
	/***
	 * 走廊->房间跳转
	 * @param nowPath
	 * @param lastPath
	 * @param jumpsThreshold
	 * @param tagInfor
	 * @param place
	 * @param assistLocal
	 * @return
	 */
	private final Point mainToBranchPath(BasePath nowPath, BasePath lastPath, int jumpsThreshold, FilterTagInfor tagInfor) {
		double x = -1, y = -1;
		if (isSkip(lastPath, nowPath.getId())) { 
		  // 直接跳转就可以了, 第一次在开始位置
      	  tagInfor.setPid(nowPath.getId());
      	  tagInfor.setPhops(jumpsThreshold);
      	  x = nowPath.getStart().getX();
      	  y = nowPath.getStart().getY();
          if (isFilter()) {
          	  tagInfor.setKx(new KalmanFilter(x, getFilterArg()));
          	  tagInfor.setKy(new KalmanFilter(y, getFilterArg()));
          }
        } else { // 不可以直接发生跳转
            if (tagInfor.getPhops() <= 0) {
          	  tagInfor.setPid(nowPath.getId());
          	  tagInfor.setPhops(jumpsThreshold);
          	  x = nowPath.getStart().getX();
          	  y = nowPath.getStart().getY();
              if (isFilter()) {
              	 tagInfor.setKx(new KalmanFilter(x, getFilterArg()));
              	 tagInfor.setKy(new KalmanFilter(y, getFilterArg()));
              }
            } else {
          	  tagInfor.setPhops(tagInfor.getPhops() - 1);
          	  return new Point(-1, - 1);
            }
        }
		return new Point(x, y);
	}
	
	/***
	 * 走廊->走廊跳转
	 * @param nowPath
	 * @param lastPath
	 * @param tagInfor
	 * @param place
	 * @param assistLocal
	 * @return
	 * @throws Exception 
	 */
	private final Point mainToMainPath(BasePath nowPath, BasePath lastPath, int jumpsThreshold, FilterTagInfor tagInfor, Point place) throws Exception {
		double x = -1, y = -1;
		if(lastPath.getId().equalsIgnoreCase(nowPath.getId())) {
			 if(isFilter()) {
				 if (null != tagInfor.getKx() && null != tagInfor.getKy()) {
					x = tagInfor.getKx().filter(place.getX());
					y = tagInfor.getKy().filter(place.getY());
                } else {
               	 tagInfor.setKx(new KalmanFilter(place.getX(), getFilterArg()));
               	 tagInfor.setKy(new KalmanFilter(place.getY(), getFilterArg()));
               	 // 此时的x, y就是place
                }
            } else {
           	// 不采用过滤时x, y就是place
            }
		 } else { // 说明当前两次路径不相同
			 if (isSkip(lastPath, nowPath.getId())) {
                // 设置当前路径和跳转以后的阀值
				tagInfor.setPid(nowPath.getId());
				tagInfor.setPhops(jumpsThreshold);
                if (isFilter()) {// 第一次跳转，我们应该重新生成坐标
               	 	tagInfor.setKx(new KalmanFilter(place.getX(), getFilterArg()));
               	 	tagInfor.setKy(new KalmanFilter(place.getY(), getFilterArg()));
                }
                // 此时的x, y就是place
            } else { 
                // 说明不能直接跳转到现在的路径
                if (tagInfor.getPhops() <= 0) {
                	// 达到了不能跳转的阀值后进行跳转
               	   tagInfor.setPid(nowPath.getId());
               	   tagInfor.setPhops(jumpsThreshold);
                   if (isFilter()) {
                   	  tagInfor.setKx(new KalmanFilter(place.getX(), getFilterArg()));
                   	  tagInfor.setKy(new KalmanFilter(place.getY(), getFilterArg()));
                   }
                   // 此时的x, y就是place
                } else {
                	// 没有达到阀值，我们需要将对应的阀值 - 1操作
               	 	tagInfor.setPhops(tagInfor.getPhops() - 1);
               	 	// 还是应该取之前的坐标
               	 	return new Point(-1, -1);
                }
            }
		}
		if(x <= 0 || y <= 0) {
			return place;
		}
		return new Point(x, y); 
	}
	
	/***
	 * 过滤可能跳点的情况
	 * @param tagInfor
	 * @param place：
	 * @param assistLocal
	 * @return
	 * @throws Exception
	 */
	public Point filterHopsCoordinate(FilterTagInfor tagInfor, Point place, AssistLocation assistLocal) throws Exception {
		int jumpsThreshold = 1;
		BasePath lastPath = null, currPath = null;
		currPath = assistLocal.getAssistPath();
        if(!StringUtil.isEmpty(tagInfor.getPid())) {
        	lastPath = getPathsMap().get(tagInfor.getPid());
        }
        jumpsThreshold = obtainJumpsThreshold(tagInfor.getSleepTime());
        if (null != lastPath) {
           if (null != currPath) { // 当前的路径不为空
              // 说明上一次的路径和当前路径都不为空，有4中组合
        	  if(lastPath instanceof MainPath && currPath instanceof MainPath) {
        		  return mainToMainPath(currPath, lastPath, jumpsThreshold, tagInfor, place);
        	  } else if(lastPath instanceof MainPath && currPath instanceof BranchPath) {
                  return mainToBranchPath(currPath, lastPath, jumpsThreshold, tagInfor);
        	  } else if(lastPath instanceof BranchPath && currPath instanceof MainPath) {
                  return branchToMainPath(currPath, lastPath, jumpsThreshold, tagInfor, place);
        	  } else {
                  return branchToBranchPath(currPath, lastPath, jumpsThreshold, tagInfor, place);
        	  }
            } else  {
            	return new Point(-1, -1);
            }
        } else { 
            // 说明上一次的路径为空，我们应该直接跳转到当前路径上
        	if(null != currPath) {
        		tagInfor.setPid(currPath.getId());
        		tagInfor.setPhops(jumpsThreshold);
        		if (isFilter()) {
                    // 采用滤波我们此时要生成kalman初始值
                    tagInfor.setKx(new KalmanFilter(place.getX()));
                    tagInfor.setKy(new KalmanFilter(place.getY()));
                }
        		return place;
        	}
        }
		return new Point(-1, -1);
	}
	/***
	 * 进行路径切换时，判断是否可以直接进行路径的切换
	 * @param path
	 * @param id
	 * @return
	 */
	private final boolean isSkip(BasePath path, String id) {
		for(String item:path.getBindPaths()) {
            if (item.equalsIgnoreCase(id)) {
                return true;
            }
        }
        return false;
	}
	public boolean isFilter() {
		return isFilter;
	}
	public void setFilter(boolean isFilter) {
		this.isFilter = isFilter;
	}
	public double getFilterArg() {
		return filterArg;
	}
	public void setFilterArg(double filterArg) {
		this.filterArg = filterArg;
	}
	public Config getConfig() {
		return config;
	}
	public void setConfig(Config config) {
		this.config = config;
	}
	public Map<String, BasePath> getPathsMap() {
		return pathsMap;
	}
	public void setPathsMap(Map<String, BasePath> pathsMap) {
		this.pathsMap = pathsMap;
	}
}
